﻿// Copyright (c) 2012, Alexander Endris
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
// NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#region Usings

using System;
using System.Collections.Generic;
using System.Drawing;
using System.IO;
using NamelessOne.Imaging.Compression;
using NamelessOne.Imaging.IO;
using BinaryReader = NamelessOne.Imaging.IO.BinaryReader;

#endregion

namespace NamelessOne.Imaging.Jpg
{
  /// <summary>
  ///   An Image in JPEG Format
  /// </summary>
  internal class JpgImage : Image
  {
    #region Public / Methods

    /// <summary>
    ///   Führt anwendungsspezifische Aufgaben durch, die mit der Freigabe, der Zurückgabe oder dem Zurücksetzen von nicht verwalteten Ressourcen zusammenhängen.
    /// </summary>
    /// <filterpriority>2</filterpriority>
    public override void Dispose()
    {
      pixels = null;
    }

    /// <summary>
    ///   Gets the color values of the specified pixel
    /// </summary>
    /// <param name = "x">X-Coordinate of the pixel</param>
    /// <param name = "y">Y-Coordinate of the pixel</param>
    /// <returns>Color value of the specified pixel</returns>
    public override Color16 GetPixel(uint x, uint y)
    {
      return pixels[x, y];
    }

    /// <summary>
    ///   Reads the next line of pixels and outputs it as a byte array.
    ///   The PixelFormat property has to be taken into account so the values in the byte array can be interpreted correcly
    /// </summary>
    /// <param name = "scanline">A byte array that contains the raw data of the pixels in the scanline. If there is no more scanline the byte array will have a length of zero</param>
    /// <returns>True if there was another scanline, false if there wasn't</returns>
    public override bool ReadNextScanline(out byte[] scanline)
    {
      if (currentScanline == height)
      {
        scanline = new byte[0];
        return false;
      }

      scanline = new byte[width*3];
      for (int i = 0; i < width; i++)
      {
        Color c = pixels[i, currentScanline];
        scanline[i*3] = c.R;
        scanline[i*3 + 1] = c.G;
        scanline[i*3 + 2] = c.B;
      }
      currentScanline++;
      return true;
    }

    /// <summary>
    ///   Resets the internal scanline counter and streams so ReadNextScanline starts at the first scanline in the image.
    /// </summary>
    public override void ResetScanline()
    {
      currentScanline = 0;
    }

    /// <summary>
    ///   Not yet implemented and supported
    /// </summary>
    /// <param name = "x"></param>
    /// <param name = "y"></param>
    /// <param name = "value"></param>
    public override void SetPixel(uint x, uint y, Color16 value)
    {
      throw new NotImplementedException();
    }

    #endregion

    #region Private / Attributes

    /// For precalculated dct --- Not used here
    //private static readonly double C1 = Math.Sqrt(2)*Math.Cos(Math.PI/16.0);
    //private static readonly double C2 = Math.Sqrt(2)*Math.Cos(2*Math.PI/16.0);
    //private static readonly double C3 = Math.Sqrt(2)*Math.Cos(3*Math.PI/16.0);
    //private static readonly double ReciSqrt2 = 1.0/Math.Sqrt(2);
    //private static readonly double S1 = Math.Sqrt(2)*Math.Sin(Math.PI/16.0);
    //private static readonly double S2 = Math.Sqrt(2)*Math.Sin(2*Math.PI/16.0);
    //private static readonly double S3 = Math.Sqrt(2)*Math.Sin(3*Math.PI/16.0);
    private static readonly double[,] dct = CalculateDct();

    private static readonly double[,] dctT = CalculateDctT();

    // Start of Huffman-Trees
    private static readonly int dht = 0xFFC4;
    // Start of Quantization Matrices
    private static readonly int dqt = 0xFFDB;
    // Definition of restart markers
    private static readonly int dri = 0xFFDD;
    // Restart marker (inside the scan data)
    private static readonly int rst = 0xFFD0;
    // Start of Frame (sequential)
    private static readonly int sof0 = 0xFFC0;
    // Start of Frame (progressive)
    private static readonly int sof2 = 0xFFC2;
    // Start of Image
    private static readonly int soi = 0xFFD8;
    // Start of Scan (Image Data)
    private static readonly int sos = 0xFFDA;

    private static readonly int[] unZigZag = new[]
                                               {
                                                 0, 1, 8, 16, 9, 2, 3, 10,
                                                 17, 24, 32, 25, 18, 11, 4, 5,
                                                 12, 19, 26, 33, 40, 48, 41, 34,
                                                 27, 20, 13, 6, 7, 14, 21, 28,
                                                 35, 42, 49, 56, 57, 50, 43, 36,
                                                 29, 22, 15, 23, 30, 37, 44, 51,
                                                 58, 59, 52, 45, 38, 31, 39, 46,
                                                 53, 60, 61, 54, 47, 55, 62, 63
                                               };

    private HuffmanTree chromaAc;
    private HuffmanTree chromaDc;
    private byte[,] chromaQ;
    private int currentScanline;
    private HuffmanTree lumaAc;
    private HuffmanTree lumaDc;
    private byte[,] lumaQ;
    private int mcuInterval;
    private Color[,] pixels;
    private int prevCbDc;
    private int prevCrDc;
    private int prevYDc;
    private bool restartMarkers;

    private Subsampling subsampling;

    #endregion

    #region Private / Constructors

    internal JpgImage(string fileName)
    {
      this.fileName = fileName;
      imageFormat = ImageFormat.Jpg;
      pixelFormat = PixelFormat.Rgb24;
      Initialize(fileName);
    }

    #endregion

    #region Private / Methods

    /// <summary>
    ///   Pre-Calculates the DCT Function
    /// </summary>
    private static double[,] CalculateDct()
    {
      double[,] result = new double[8,8];

      for (int y = 0; y < 8; y++)
      {
        for (int x = 0; x < 8; x++)
        {
          double value;

          if (y == 0)
            value = Math.Sqrt(1/8f);
          else
            value = Math.Sqrt(2f/8f)*Math.Cos(((2*x + 1)*y*Math.PI)/16);
          result[x, y] = value;
        }
      }
      return result;
    }

    /// <summary>
    ///   Pre-Calculates the Transpose of the DCT Function
    /// </summary>
    private static double[,] CalculateDctT()
    {
      double[,] result = new double[8,8];

      for (int x = 0; x < 8; x++)
      {
        for (int y = 0; y < 8; y++)
        {
          double value;

          if (x == 0)
            value = Math.Sqrt(1/8f);
          else
            value = Math.Sqrt(2f/8f)*Math.Cos(((2*y + 1)*x*Math.PI)/16);
          result[x, y] = value;
        }
      }
      return result;
    }

    /// <summary>
    ///   Clamps a value into 0 and 255
    /// </summary>
    private static byte Clamp(long value)
    {
      return (byte) Math.Min(255, Math.Max(value, 0));
    }

    //private static byte[,] FactorizedIDCT(short[,] block)
    //{
    //  byte[,] result = new byte[8,8];
    //  double[,] temp = new double[8,8];
    //  for (int i = 0; i < 8; i++)
    //  {
    //    double a0 = block[i, 0];
    //    double a1 = block[i, 4];
    //    double a2 = block[i, 2];
    //    double a3 = block[i, 6];
    //    double a4 = block[i, 1];
    //    double a5 = block[i, 5];
    //    double a6 = block[i, 3];
    //    double a7 = block[i, 7];
    //    double b0 = a0 + a1;
    //    double b1 = a0 - a1;
    //    double r0 = S2*(a2 + a3);
    //    double b2 = r0 - (S2 + C2)*a3;
    //    double b3 = r0 - (S2 - C2)*a2;
    //    double r1 = S1*(a4 + a7);
    //    double b4 = r1 - (S1 + C1)*a7;
    //    double b7 = r1 - (S1 - C1)*a4;
    //    double r2 = C3*(a5 + a6);
    //    double b5 = r2 - (C3 + S3)*a6;
    //    double b6 = r2 - (C3 - S3)*a5;
    //    double c5 = b5 - b4;
    //    double c6 = b7 - b6;
    //    double d5 = c6 + c5;
    //    double d6 = c6 - c5;
    //    double f0 = b0 + b3;
    //    double f1 = b1 + b2;
    //    double f2 = b1 - b2;
    //    double f3 = b0 - b3;
    //    double f4 = b4 + b5;
    //    double f5 = ReciSqrt2*d5;
    //    double f6 = ReciSqrt2*d6;
    //    double f7 = b7 + b6;
    //    temp[i, 0] = f0 + f7;
    //    temp[i, 1] = f1 + f6;
    //    temp[i, 2] = f2 + f5;
    //    temp[i, 3] = f3 + f4;
    //    temp[i, 4] = f3 - f4;
    //    temp[i, 5] = f2 - f5;
    //    temp[i, 6] = f1 - f6;
    //    temp[i, 7] = f0 - f7;
    //  }
    //  for (int i = 0; i < 8; i++)
    //  {
    //    double a0 = temp[0, i];
    //    double a1 = temp[4, i];
    //    double a2 = temp[2, i];
    //    double a3 = temp[6, i];
    //    double a4 = temp[1, i];
    //    double a5 = temp[5, i];
    //    double a6 = temp[3, i];
    //    double a7 = temp[7, i];
    //    double b0 = a0 + a1;
    //    double b1 = a0 - a1;
    //    double r0 = S2*(a2 + a3);
    //    double b2 = r0 - (S2 + C2)*a3;
    //    double b3 = r0 - (S2 - C2)*a2;
    //    double r1 = S1*(a4 + a7);
    //    double b4 = r1 - (S1 + C1)*a7;
    //    double b7 = r1 - (S1 - C1)*a4;
    //    double r2 = C3*(a5 + a6);
    //    double b5 = r2 - (C3 + S3)*a6;
    //    double b6 = r2 - (C3 - S3)*a5;
    //    double c5 = b5 + b4;
    //    double c6 = b7 + b6;
    //    double d5 = c6 + c5;
    //    double d6 = c6 - c5;
    //    double e0 = b0 + (128*8);
    //    double e1 = b1 + (128*8);
    //    double f0 = e0 + b3;
    //    double f1 = e1 + b2;
    //    double f2 = e1 - b2;
    //    double f3 = e0 - b3;
    //    double f4 = b4 + b5;
    //    double f5 = ReciSqrt2*d5;
    //    double f6 = ReciSqrt2*d6;
    //    double f7 = b7 + b6;
    //    double g0 = (f0 + f7)/8.0;
    //    double g1 = (f1 + f6)/8.0;
    //    double g2 = (f2 + f5)/8.0;
    //    double g3 = (f3 + f4)/8.0;
    //    double g4 = (f3 - f4)/8.0;
    //    double g5 = (f2 - f5)/8.0;
    //    double g6 = (f1 - f6)/8.0;
    //    double g7 = (f0 - f7)/8.0;
    //    result[0, i] = Clamp((long) Math.Round(g0));
    //    result[1, i] = Clamp((long) Math.Round(g1));
    //    result[2, i] = Clamp((long) Math.Round(g2));
    //    result[3, i] = Clamp((long) Math.Round(g3));
    //    result[4, i] = Clamp((long) Math.Round(g4));
    //    result[5, i] = Clamp((long) Math.Round(g5));
    //    result[6, i] = Clamp((long) Math.Round(g6));
    //    result[7, i] = Clamp((long) Math.Round(g7));
    //  }
    //  return result;
    //}
    /// Factorized DCT for optimization --- Not used here and not really functional as it is here
    /// <summary>
    ///   Linear interpolation of a two bytes
    /// </summary>
    private static byte Lerp(byte left, byte right, float factor)
    {
      byte result = (byte) (left + (right - left)*factor);
      return result;
    }

    /// <summary>
    ///   Calculation of the IDCT using Matrix multiplication
    /// </summary>
    private static byte[,] MatrixMultiplicationIDCT(short[,] block)
    {
      short[,] sResult = Mul(dctT, block);
      byte[,] result = Mul(sResult, dct);
      return result;
    }

    /// <summary>
    ///   Matrix Multiplication Block with the DCT-Transpose
    /// </summary>
    private static byte[,] Mul(short[,] block, double[,] dctT)
    {
      byte[,] result = new byte[8,8];

      for (int y = 0; y < 8; y++)
      {
        for (int x = 0; x < 8; x++)
        {
          double value = 0;
          for (int i = 0; i < 8; i++)
            value += block[i, y]*dctT[x, i];
          result[x, y] = (byte) Math.Min(Math.Max(0, value + 128), 255);
        }
      }

      return result;
    }

    /// <summary>
    ///   Matrix Multiplication DCT with the Block
    /// </summary>
    private static short[,] Mul(double[,] dct, short[,] block)
    {
      short[,] result = new short[8,8];

      for (int y = 0; y < 8; y++)
      {
        for (int x = 0; x < 8; x++)
        {
          double value = 0;
          for (int i = 0; i < 8; i++)
            value += dct[i, y]*block[x, i];
          result[x, y] = (short) Math.Round(value);
        }
      }

      return result;
    }

    /// <summary>
    ///   Helper Method that seeks to the specified tag
    /// </summary>
    private static bool SeekTo(BinaryReader reader, int seekTo)
    {
      reader.Seek(2, SeekOrigin.Begin);
      byte ff = 0xFF;
      byte code = 0;
      seekTo &= 0xFF;

      byte b = reader.ReadByte();

      while (reader.Peek() != -1)
      {
        if (b == ff)
        {
          code = reader.ReadByte();
          if (code == seekTo)
            return true;
          else
          {
            ushort length = reader.ReadUInt16();

            reader.Seek(length - 2, SeekOrigin.Current);
          }
        }

        try
        {
          b = reader.ReadByte();
        }
        catch (Exception e)
        {
          break;
        }
      }
      return false;
    }

    //public List<HuffmanTree> CreateHuffmanTrees(BinaryReader reader)
    //{
    //  List<HuffmanTree> trees = new List<HuffmanTree>();
    //  reader.Seek(2, SeekOrigin.Begin);
    //  while (SeekToNext(reader, dht) && trees.Count < 4)
    //  {
    //    Dictionary<int, List<byte>> values = new Dictionary<int, List<byte>>(16);
    //    short count = (short)(reader.ReadInt16() - 2);
    //    long start = reader.Position;
    //    while (start + count > reader.Position)
    //    {
    //      byte type = reader.ReadByte();
    //      int[] counts = new int[16];
    //      int sum = 0;
    //      for (int i = 0; i < 16; i++)
    //      {
    //        sum += counts[i] = reader.ReadByte();
    //        values[i + 1] = new List<byte>();
    //      }

    //      for (int i = 0; i < 16; i++)
    //      {
    //        int c = counts[i];

    //        for (int j = 0; j < c; j++)
    //        {
    //          values[i + 1].Add(reader.ReadByte());
    //        }
    //      }

    //      HuffmanTree result = new HuffmanTree(new BinaryNode(), type);

    //      int code = 0;
    //      int nodeCount = 1;
    //      foreach (var row in values)
    //      {
    //        int bitcount = row.Key;
    //        nodeCount = nodeCount << 1;

    //        var v = row.Value;

    //        int intermediateCode = code;
    //        for (int i = 0; i < nodeCount && sum != 0; i++)
    //        {
    //          BinaryNode node = result.Root;
    //          for (int bitPos = 0; bitPos < bitcount; bitPos++)
    //          {
    //            bool bit = BitReader.GetBitValue(intermediateCode, bitcount, bitPos);

    //            node = bit ? node.One : node.Zero;
    //          }

    //          if (i < v.Count)
    //          {
    //            node.Value = v[i];
    //            sum--;
    //          }
    //          else
    //          {
    //            node.Zero = new BinaryNode();
    //            node.One = new BinaryNode();
    //          }

    //          intermediateCode++;
    //        }
    //        if (sum == 0)
    //          break;
    //        nodeCount -= v.Count;
    //        code = (code + counts[bitcount - 1]) << 1;
    //      }

    //      trees.Add(result);
    //    }
    //  }
    //  return trees;
    //}

    /// <summary>
    ///   Helper Method that seeks to the next occurence of the specified tag
    /// </summary>
    private static bool SeekToNext(BinaryReader reader, int seekTo)
    {
      byte ff = 0xFF;
      byte code = 0;
      seekTo &= 0xFF;

      byte b = reader.ReadByte();

      while (reader.Peek() != -1)
      {
        if (b == ff)
        {
          code = reader.ReadByte();
          if (code == seekTo)
            return true;
          if (code == (sos & 0xff))
            return false;

          ushort length = reader.ReadUInt16();

          reader.Seek(length - 2, SeekOrigin.Current);
        }

        b = reader.ReadByte();
      }
      return false;
    }

    //private short CalculateIDct(int x, int y, short[,] block)
    //{
    //  double sum = 0;
    //  for (int v = 0; v < 8; v++)
    //  {
    //    for (int u = 0; u < 8; u++)
    //    {
    //      double au = (u == 0 ? Math.Sqrt(1f/8f) : Math.Sqrt(2f/8f));
    //      double av = (v == 0 ? Math.Sqrt(1f/8f) : Math.Sqrt(2f/8f));
    //      double value = block[u, v];
    //      double cosX = Math.Cos((Math.PI/8)*(x + 0.5f)*u);
    //      double cosY = Math.Cos((Math.PI/8)*(y + 0.5f)*v);
    //      sum += au*av*value*cosX*cosY;
    //    }
    //  }
    //  return (short) Math.Max(Math.Min(127, Math.Round(sum)), -128);
    //}
    /// <summary>
    ///   Calculates the IDCT on the fly --- not used here as it is very slow
    /// </summary>
    /// <summary>
    ///   Reads the huffman tables and creates the underlying tree structure for decoding
    /// </summary>
    private List<HuffmanTree> CreateHuffmanTrees(BinaryReader reader)
    {
      List<HuffmanTree> trees = new List<HuffmanTree>();
      reader.Seek(2, SeekOrigin.Begin);
      while (SeekToNext(reader, dht) && trees.Count < 4)
      {
        short count = (short) (reader.ReadInt16() - 2);
        long start = reader.Position;
        while (start + count > reader.Position)
        {
          byte[] values = new byte[256];
          byte type = reader.ReadByte();
          byte[] counts = new byte[16];
          int sum = 0;
          for (int i = 0; i < 16; i++)
            sum += counts[i] = reader.ReadByte();

          for (int j = 0; j < sum; j++)
            values[j] = reader.ReadByte();

          HuffmanTree result = new HuffmanTree(values, counts, type);

          trees.Add(result);
        }
      }
      return trees;
    }

    /// <summary>
    ///   Dequantization process of decoding a jpg image
    /// </summary>
    private short[,] DeQuantize(short[,] block, byte[,] quantizer)
    {
      short[,] result = new short[8,8];

      for (int y = 0; y < 8; y++)
      {
        for (int x = 0; x < 8; x++)
          result[x, y] = (short) (block[x, y]*quantizer[x, y]);
      }

      return result;
    }

    /// <summary>
    ///   Decodes a MCU without subsampling
    /// </summary>
    private void Decode1x1(BitReader reader, out byte[,] y00, out byte[,] cb, out byte[,] cr)
    {
      y00 = UnDct(DeQuantize(DecodeBlock(reader, lumaDc, lumaAc, ref prevYDc), lumaQ));
      cb = UnDct(DeQuantize(DecodeBlock(reader, chromaDc, chromaAc, ref prevCbDc), chromaQ));
      cr = UnDct(DeQuantize(DecodeBlock(reader, chromaDc, chromaAc, ref prevCrDc), chromaQ));
    }

    /// <summary>
    ///   Decodes a MCU with 4:4:0 subsampling
    /// </summary>
    private void Decode1x2(BitReader reader, out byte[,] y00, out byte[,] y01, out byte[,] cb, out byte[,] cr)
    {
      y00 = UnDct(DeQuantize(DecodeBlock(reader, lumaDc, lumaAc, ref prevYDc), lumaQ));
      y01 = UnDct(DeQuantize(DecodeBlock(reader, lumaDc, lumaAc, ref prevYDc), lumaQ));

      cb = UnDct(DeQuantize(DecodeBlock(reader, chromaDc, chromaAc, ref prevCbDc), chromaQ));
      cr = UnDct(DeQuantize(DecodeBlock(reader, chromaDc, chromaAc, ref prevCrDc), chromaQ));
    }

    /// <summary>
    ///   Decodes a MCU with 4:2:2 subsampling
    /// </summary>
    private void Decode2x1(BitReader reader, out byte[,] y00, out byte[,] y10, out byte[,] cb, out byte[,] cr)
    {
      y00 = UnDct(DeQuantize(DecodeBlock(reader, lumaDc, lumaAc, ref prevYDc), lumaQ));
      y10 = UnDct(DeQuantize(DecodeBlock(reader, lumaDc, lumaAc, ref prevYDc), lumaQ));

      cb = UnDct(DeQuantize(DecodeBlock(reader, chromaDc, chromaAc, ref prevCbDc), chromaQ));
      cr = UnDct(DeQuantize(DecodeBlock(reader, chromaDc, chromaAc, ref prevCrDc), chromaQ));
    }

    /// <summary>
    ///   Decodes a MCU with 4:2:0 subsampling
    /// </summary>
    private void Decode2x2(BitReader reader, out byte[,] y00, out byte[,] y10, out byte[,] y01, out byte[,] y11,
                           out byte[,] cb, out byte[,] cr)
    {
      y00 = UnDct(DeQuantize(DecodeBlock(reader, lumaDc, lumaAc, ref prevYDc), lumaQ));
      y10 = UnDct(DeQuantize(DecodeBlock(reader, lumaDc, lumaAc, ref prevYDc), lumaQ));
      y01 = UnDct(DeQuantize(DecodeBlock(reader, lumaDc, lumaAc, ref prevYDc), lumaQ));
      y11 = UnDct(DeQuantize(DecodeBlock(reader, lumaDc, lumaAc, ref prevYDc), lumaQ));

      cb = UnDct(DeQuantize(DecodeBlock(reader, chromaDc, chromaAc, ref prevCbDc), chromaQ));
      cr = UnDct(DeQuantize(DecodeBlock(reader, chromaDc, chromaAc, ref prevCrDc), chromaQ));
    }

    /// <summary>
    ///   Decodes a MCU with huffman coding
    /// </summary>
    private short[,] DecodeBlock(BitReader reader, HuffmanTree dc, HuffmanTree ac, ref int prevDc)
    {
      short[,] data = new short[8,8];
      for (int i = 0; i < 64; i++)
      {
        byte magnitude;
        byte zeroRun = 0;

        if (i == 0)
          magnitude = (byte) dc.Decode(reader);
        else
        {
          byte value = (byte) ac.Decode(reader);

          magnitude = (byte) (value & 0x0F);
          zeroRun = (byte) ((value & 0xF0) >> 4);

          if (magnitude == 0)
          {
            if (zeroRun == 0xF)
            {
              i += 15;
              continue;
            }
            if (zeroRun == 0)
              break;
          }
        }

        short additional = 0;
        i += zeroRun;
        int index = unZigZag[i];

        for (int j = 0; j < magnitude; j++)
        {
          bool bit = reader.ReadBit();
          additional |= (short) ((bit ? 1 : 0) << (magnitude - j - 1));
        }
        short v = Extend(additional, magnitude);
        if (i == 0)
          prevDc = data[index%8, index/8] = (short) (v + prevDc);
        else
        {
          data[index%8, index/8] = v;

          if (data[index%8, index/8] == 0)
            break;
        }
      }
      return data;
    }

    /// <summary>
    ///   Processes the entire image and decodes it
    /// </summary>
    private void DecodeScanData(BinaryReader reader)
    {
      SeekTo(reader, sos);
      int length = reader.ReadUInt16();
      reader.Seek(length - 2, SeekOrigin.Current);
      pixels = new Color[width,height];
      BitReader bitReader = new BitReader(reader);

      int x = 0;
      int y = 0;
      int xChroma = 0;
      int yChroma = 0;
      int hUpscale = 1;
      int vUpscale = 1;

      byte[,] yValues = null;
      byte[,] cbValues = null;
      byte[,] crValues = null;

      byte[,] y00;
      byte[,] y10;
      byte[,] y01;
      byte[,] y11;
      byte[,] cb;
      byte[,] cr;

      int mcuCount = 0;
      byte restartMarker = 0;

      while (y < height)
      {
        int offsetX = 0;
        int offsetY = 0;

        if (restartMarkers && mcuCount > 0 && mcuCount%mcuInterval == 0)
        {
          bitReader.Reset();
          ushort marker = reader.ReadUInt16();
          if ((marker & 0xFFF0) == rst)
          {
            if (restartMarker != (marker & 0xF))
              throw new InvalidDataException();
            restartMarker = (byte) ((restartMarker + 1)%8);
            prevYDc = 0;
            prevCbDc = 0;
            prevCrDc = 0;
          }
        }

        switch (subsampling)
        {
          case Subsampling.None:
            if (yValues == null)
            {
              hUpscale = 1;
              vUpscale = 1;
              int w = 8*((int) ((float) width/8) + 1);
              int h = 8*((int) ((float) height/8) + 1);

              yValues = new byte[w,h];
              cbValues = new byte[w,h];
              crValues = new byte[w,h];
            }

            Decode1x1(bitReader, out y00, out cb, out cr);

            for (offsetY = 0; offsetY < 8; offsetY++)
            {
              for (offsetX = 0; offsetX < 8; offsetX++)
              {
                yValues[x + offsetX, y + offsetY] = y00[offsetX, offsetY];
                cbValues[x + offsetX, y + offsetY] = cb[offsetX, offsetY];
                crValues[x + offsetX, y + offsetY] = cr[offsetX, offsetY];
              }
            }

            x += 8;
            if (x >= width)
            {
              x = 0;
              y += 8;
            }
            break;
          case Subsampling.Horizontal:
            if (yValues == null)
            {
              hUpscale = 1;
              vUpscale = 1;
              int w = 8*((int) ((float) width/8) + 1);
              int h = 8*((int) ((float) height/8) + 1);

              yValues = new byte[w,h];
              cbValues = new byte[w,h];
              crValues = new byte[w,h];
            }

            Decode2x1(bitReader, out y00, out y10, out cb, out cr);

            for (offsetY = 0; offsetY < 8; offsetY++)
            {
              for (offsetX = 0; offsetX < 16; offsetX++)
              {
                if (offsetX < 8)
                {
                  yValues[x + offsetX, y + offsetY] = y00[offsetX, offsetY];
                  cbValues[x + offsetX, y + offsetY] = cb[offsetX, offsetY];
                  crValues[x + offsetX, y + offsetY] = cr[offsetX, offsetY];
                }
                else
                  yValues[x + offsetX, y + offsetY] = y10[offsetX - 8, offsetY];
              }
            }

            x += 16;
            if (x >= width)
            {
              x = 0;
              y += 8;
            }
            break;
          case Subsampling.Vertical:
            if (yValues == null)
            {
              hUpscale = 1;
              vUpscale = 1;
              int w = 8*((int) ((float) width/8) + 1);
              int h = 8*((int) ((float) height/8) + 1);

              yValues = new byte[w,h];
              cbValues = new byte[w,h];
              crValues = new byte[w,h];
            }

            Decode1x2(bitReader, out y00, out y01, out cb, out cr);

            for (offsetY = 0; offsetY < 16; offsetY++)
            {
              for (offsetX = 0; offsetX < 8; offsetX++)
              {
                if (offsetY < 8)
                {
                  yValues[x + offsetX, y + offsetY] = y00[offsetX, offsetY];
                  cbValues[x + offsetX, y + offsetY] = cb[offsetX, offsetY];
                  crValues[x + offsetX, y + offsetY] = cr[offsetX, offsetY];
                }
                else
                  yValues[x + offsetX, y + offsetY] = y01[offsetX, offsetY - 8];
              }
            }

            x += 8;
            if (x >= width)
            {
              x = 0;
              y += 16;
            }
            break;
          case Subsampling.HorizontalVertical:
            if (yValues == null)
            {
              hUpscale = 2;
              vUpscale = 2;
              int w = 16*((int) ((float) width/16) + 1);
              int h = 16*((int) ((float) height/16) + 1);

              yValues = new byte[w,h];
              cbValues = new byte[w/2,h/2];
              crValues = new byte[w/2,h/2];
            }

            Decode2x2(bitReader, out y00, out y10, out y01, out y11, out cb, out cr);

            for (offsetY = 0; offsetY < 16; offsetY++)
            {
              for (offsetX = 0; offsetX < 16; offsetX++)
              {
                if (offsetX < 8)
                {
                  if (offsetY < 8)
                  {
                    yValues[x + offsetX, y + offsetY] = y00[offsetX, offsetY];
                    cbValues[xChroma + offsetX, yChroma + offsetY] = cb[offsetX, offsetY];
                    crValues[xChroma + offsetX, yChroma + offsetY] = cr[offsetX, offsetY];
                  }
                  else
                    yValues[x + offsetX, y + offsetY] = y01[offsetX, offsetY - 8];
                }
                else
                {
                  if (offsetY < 8)
                    yValues[x + offsetX, y + offsetY] = y10[offsetX - 8, offsetY];
                  else
                    yValues[x + offsetX, y + offsetY] = y11[offsetX - 8, offsetY - 8];
                }
              }
            }

            x += 16;
            xChroma += 8;
            if (x >= width)
            {
              x = 0;
              xChroma = 0;
              y += 16;
              yChroma += 8;
            }

            break;
          default:
            throw new ArgumentOutOfRangeException();
        }
        mcuCount++;
      }

      // Upscaling if there is chroma subsampling
      cbValues = Upscale(cbValues, hUpscale, vUpscale);
      crValues = Upscale(crValues, hUpscale, vUpscale);

      for (y = 0; y < height; y++)
      {
        for (x = 0; x < width; x++)
          pixels[x, y] = new YCbCr(yValues[x, y], cbValues[x, y], crValues[x, y]);
      }
    }

    /// <summary>
    ///   Extends the additional bits from the huffmancoding
    /// </summary>
    private short Extend(short additional, byte magnitude)
    {
      short tester = (short) (1 << (magnitude - 1));

      if (additional < tester)
        return (short) (additional + (-1 << magnitude) + 1);

      return additional;
    }

    /// <summary>
    ///   Initializes the image
    /// </summary>
    private void Initialize(string fileName)
    {
      using (Stream str = new FileStream(fileName, FileMode.Open, FileAccess.Read, FileShare.ReadWrite))
      using (BinaryReader reader = new BinaryReader(str, true))
      {
        ReadQuantizationTables(reader);
        ReadDCT(reader);
        ReadDri(reader);
        ReadHuffmanTables(reader);
        DecodeScanData(reader);
      }
    }

    //private byte[,] LongVersionIDCT(short[,] block)
    //{
    //  byte[,] result = new byte[8,8];
    //  for (int y = 0; y < 8; y++)
    //  {
    //    for (int x = 0; x < 8; x++)
    //      result[x, y] = (byte) (CalculateIDct(x, y, block) + 128);
    //  }
    //  return result;
    //}
    /// <summary>
    ///   Calculates the IDCT on the fly -- Not used here, since it is very slow
    /// </summary>
    /// <summary>
    ///   Reads width, heigt and subsampling
    /// </summary>
    private void ReadBaselineDCT(BinaryReader reader)
    {
      ushort length = reader.ReadUInt16();
      byte precision = reader.ReadByte();
      height = reader.ReadUInt16();
      width = reader.ReadUInt16();
      byte numComponents = reader.ReadByte();

      byte[] hSamplings = new byte[numComponents];
      byte[] vSamplings = new byte[numComponents];

      for (int i = 0; i < numComponents; i++)
      {
        byte id = reader.ReadByte();
        byte sampling = reader.ReadByte();
        reader.ReadByte();

        hSamplings[i] = (byte) ((sampling & 0xF0) >> 4);
        vSamplings[i] = (byte) (sampling & 0xF);
      }

      if (hSamplings[0] > hSamplings[numComponents - 1])
      {
        if (vSamplings[0] > vSamplings[numComponents - 1])
          subsampling = Subsampling.HorizontalVertical;
        else
          subsampling = Subsampling.Horizontal;
      }
      else if (hSamplings[0] > hSamplings[numComponents - 1])
        subsampling = Subsampling.Vertical;
      else
        subsampling = Subsampling.None;
    }

    /// <summary>
    ///   Reads DCT Chunk (currently only sequential jpg is supported)
    /// </summary>
    /// <param name = "reader"></param>
    private void ReadDCT(BinaryReader reader)
    {
      if (SeekTo(reader, sof0))
        ReadBaselineDCT(reader);
      else
        throw new NotSupportedException();
    }

    /// <summary>
    ///   Reads if there are restart markers in the image
    /// </summary>
    private void ReadDri(BinaryReader reader)
    {
      if (SeekTo(reader, dri))
      {
        ushort length = reader.ReadUInt16();
        mcuInterval = reader.ReadUInt16();
        restartMarkers = true;
      }
    }

    /// <summary>
    ///   Saves the huffman tables read from the file
    /// </summary>
    private void ReadHuffmanTables(BinaryReader reader)
    {
      reader.Seek(0, SeekOrigin.Begin);
      var trees = CreateHuffmanTrees(reader);

      foreach (var tree in trees)
      {
        if (tree.Id == 0)
        {
          if (tree.Type == 0)
            lumaDc = tree;
          else
            lumaAc = tree;
        }
        else if (tree.Id == 1)
        {
          if (tree.Type == 0)
            chromaDc = tree;
          else
            chromaAc = tree;
        }
      }
    }

    /// <summary>
    ///   Reads the quantization tables
    /// </summary>
    private void ReadQuantizationTables(BinaryReader reader)
    {
      reader.Seek(0, SeekOrigin.Begin);

      SeekTo(reader, dqt);
      ushort count = reader.ReadUInt16();

      for (int i = 0; i < 2; i++)
      {
        byte precision = reader.ReadByte();
        byte id = (byte) (precision & 0xF);
        precision = (byte) ((precision & 0xF0) >> 4);

        if (precision != 0)
          throw new NotSupportedException();

        if (id == 0)
          lumaQ = UnZigZag(reader.ReadBytes(64));
        else if (id == 1)
          chromaQ = UnZigZag(reader.ReadBytes(64));

        if (count - 67 > 0)
          continue;

        if (i == 0)
        {
          SeekToNext(reader, dqt);
          count = reader.ReadUInt16();
        }
      }
    }

    /// <summary>
    ///   Performs the IDCT on the MCU
    /// </summary>
    private byte[,] UnDct(short[,] block)
    {
      //byte[,] factorized = FactorizedIDCT(block);
      //byte[,] matrix = MatrixMultiplicationIDCT(block);
      //byte[,] calculate = LongVersionIDCT(block);
      byte[,] result = MatrixMultiplicationIDCT(block);
      return result;
    }

    /// <summary>
    ///   Reverses the zig zag of an array
    /// </summary>
    private byte[,] UnZigZag(byte[] zigzagged)
    {
      byte[,] result = new byte[8,8];

      int x = 0;
      int y = 0;
      int counter = 0;
      bool up = true;
      bool invert = false;

      while (true)
      {
        result[x, y] = zigzagged[counter];

        if (x == 7 && y == 7)
          break;

        counter++;

        if (!invert && (y == 7))
          invert = true;

        if (up)
        {
          if (y == 0 && !invert)
          {
            up = false;
            x++;
          }
          else if (x == 7 && invert)
          {
            up = false;
            y++;
          }
          else
          {
            y--;
            x++;
          }
        }
        else
        {
          if (x == 0 && !invert)
          {
            up = true;
            y++;
          }
          else if (y == 7 && invert)
          {
            up = true;
            x++;
          }
          else
          {
            x--;
            y++;
          }
        }
      }

      return result;
    }

    /// <summary>
    ///   Bilinear interpolation to compensate for the chroma subsampling
    /// </summary>
    private byte[,] Upscale(byte[,] unDct, int horizontal, int vertical)
    {
      if (horizontal == 1 && vertical == 1)
        return unDct;

      byte[,] result = new byte[unDct.GetLength(0)*horizontal,unDct.GetLength(1)*vertical];

      for (int y = 0; y < height; y++)
      {
        for (int x = 0; x < width; x++)
        {
          bool interpolateX = false;
          bool interpolateY = false;

          // Compute position in the original dataset
          float scaledX = x/(float) horizontal;
          float scaledY = y/(float) vertical;

          // if the fractional part is existent, interpolate X
          if ((int) scaledX != scaledX)
            interpolateX = true;

          // if the fractional part is existent, interpolate Y
          if ((int) scaledY != scaledY)
            interpolateY = true;

          byte resultByte;
          long offsetTopLeft;
          long offsetTopRight;

          if (interpolateX)
          {
            byte left = unDct[(int) scaledX, (int) scaledY];
            byte right = unDct[(int) (scaledX + 1 >= (width/2f) ? scaledX : scaledX + 1), (int) scaledY];

            resultByte = Lerp(left, right, scaledX - ((int) scaledX));
          }
          else
            resultByte = unDct[(int) scaledX, (int) scaledY];

          // if Y must be interpolated
          if (interpolateY)
          {
            byte bottom;
            if (interpolateX)
            {
              // Get bottom left and bottom right byte
              byte left = unDct[(int) scaledX, (int) (scaledY + 1 >= (height/2f) ? scaledY : scaledY + 1)];
              byte right =
                unDct[
                  (int) (scaledX + 1 >= (width/2f) ? scaledX : scaledX + 1),
                  (int) (scaledY + 1 >= (height/2f) ? scaledY : scaledY + 1)];

              // Interpolate
              bottom = Lerp(left, right, scaledX - ((int) scaledX));
            }
            else
              bottom = unDct[(int) scaledX, (int) (scaledY + 1 >= (height/2f) ? scaledY : scaledY + 1)];

            // Interpolate top and bottom
            resultByte = Lerp(resultByte, bottom, scaledY - ((int) scaledY));
          }

          result[x, y] = resultByte;
        }
      }
      return result;
    }

    #endregion
  }
}